package edu.wpi.first.wpilibj.robot;

import  edu.wpi.first.wpilibj.*;

public class AngleMotor extends Motor {
    Joystick confirmationJoy;
    Joystick joy; 
    GyroMotor gyro;

    int manualOverrideButtons;
    int revert;
    int supertog;
    int restButton;

    boolean isManual = true;
    boolean isResting = false;

    double sensitivityRange = .01;
    double maxAngle = 80;
    double minAngle = 0;
    
    SignalInput si;
    PrintMotor pm;

    public AngleMotor(Jaguar[] duper, Joystick driveJoy, Joystick overrideJoy, GyroMotor g, SignalInput si, Keybinds kB, PrintMotor pm) {
        super(duper);
        joy = driveJoy;
        confirmationJoy = overrideJoy;
        gyro = g;
        this.si = si;
        manualOverrideButtons = kB.manualOverride;
        revert = kB.revertToAutoAim;
        supertog = kB.liftToggle;
        restButton = kB.restButton;
        this.pm = pm;
    }
    
    
    public AngleMotor(Jaguar[] duper, Joystick driveJoy, Joystick overrideJoy, GyroMotor g, SignalInput si, PrintMotor pm) {
        this(duper,driveJoy, overrideJoy, g, si, new Keybinds(), pm);
    }
    
    public void run() {
        Jaguar theJag = theJags[1];
        while (true) {
            while (!isManual && joy.getRawButton(manualOverrideButtons) && confirmationJoy.getRawButton(manualOverrideButtons)) isManual = true;
            while (isManual && (joy.getRawButton(revert) || confirmationJoy.getRawButton(revert))) isManual = false;

            if (isManual) {
                if (confirmationJoy.getY()>.7) theJag.set(-.9);
                else if (confirmationJoy.getY()<-.7) theJag.set(.9);
                else theJag.set(0);
                pm.setText("manual", 1);
            }
            else pm.setText("autonymous", 1);
            
            
            
            theJags[0].set(0);
        }
    }
}